#ifndef RSIM_TRANSFORM_H
#define RSIM_TRANSFORM_H

/* -------------------------------------------------------------------------- *
 * File: Transform.h                                                          *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "Rotation.h"
#include "Vect3.h"

namespace RSIM{

/** Base class for Transforms */
class TransformBase{
	protected:
		/** Rotation matrix*/
		Rotation R_;
		
		/** Position vector*/
		Vect3 P_;
	public:
		/** Default constructor results in zero transform, i.e. zero
		position vector and identity rotation matrix. */
		TransformBase();
		
		/** Identity rotation matrix with position vector specified.*/
		explicit TransformBase(const Vect3& V);
		
		/** Zero position vector with rotation matrix specified*/
		explicit TransformBase(const Mat3x3& RotMat);
		
		/** @param R Rotation matrix, @param P Position vector */
		TransformBase(const Mat3x3& R, const Vect3& P);
		
		/** Return rotation matrix */
		const Rotation& R() const;
		
		Rotation& wgetR();
		
		Vect3& wgetP();
		
		/** Return position vector */
		const Vect3& P() const;
		
		/** Premultiply rotation matrix and position vector with 
		specified rotation matrix*/
		void premultiply(const Rotation& RotMat);
		
		/** Deep copy */
		void operator=(const TransformBase& T);
		
		/** Set rotation matrix */
		void setR(const Mat3x3& RotMat);
		
		/** Set position matrix */
		void setP(const Vect3& V);
}; // class TransformBase

/**
// Shift (Translate+Rotate) operator

// Consider two frames B and F which are initially coincident. On making some 
// rotations and translation the new frame F is located at a distinct location 
// from B. Since there are no restrictions on frame F, the class Tranform
// applies to any two frames and gives the location and orientation of one
// with respect to the other.

// In class Transform the 3x3 matrix R_ gives R_BF. Similarly, the 3x1 
// vector P_ locates the origin of new frame F with respect to the origin
// of frame B. That is P_ = vector(O_B to O_F) expressed in B.

*/
class Transform:public TransformBase{
	public:
		/** Default constructor sets R to identity and P_ to a 0 vector */
		Transform();
		
		/** Tranform with specified translation and identity rotation matrix */
		Transform(const Vect3& V);
		
		/** Transform with specified rotation and zero translation */
		Transform(const Mat3x3& RotMat);
		
		/** Transform with specified rotation and translation */
		Transform(const Mat3x3& R, const Vect3& P);
		
		/** Deep copy*/
		Transform(const Transform& T);
}; // class Transform

/** Class InvTransform is exactly the opposite of the class Transform as defined 
above. That is, suppose a Transform object has members R_ and P_. Then its inverse 
transform would have corresponding members as transpose(R_) and (-transpose(R_)*P_) */

class InvTransform:public TransformBase{
	private:
		InvTransform();
	public:
		/**  Set inverse of transform T */
		InvTransform(const Transform& T);
}; // class InvTransform


inline const Rotation& TransformBase::R() const {return R_;}

inline Rotation& TransformBase::wgetR(){return R_;}

inline Vect3& TransformBase::wgetP(){return P_;}

inline const Vect3& TransformBase::P() const {return P_;}

} // namespace RSIM

/** Display Transform	*/
std::ostream& operator<<(ostream& func, const RSIM::TransformBase& T);

#endif